/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: attitudeCtrl_thread.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/
#include <time.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <pthread.h>
#include <math.h>
#include <sys/netmgr.h>
#include <sys/neutrino.h>
#include <termios.h>

#include "globaldefs.h"
#include "imu.h"
#include "servos.h"
#include "utilities.h"
#include "PID.h"
#include "serial.h"

void *attitudeCtrl()
{
	//Local Variables 
	struct timespec t0,t1,t0_ant,wait,next_activation,Period;
	_uint64 auxtime;
	static struct imu *imuPacketPtr=NULL,imuPacket;
	static struct imuShort *imuShortPacketPtr=NULL,imuShortPacket;
	struct servos *servosActionPtr=NULL,servosAction;
	struct PIDCtrl *pidRollPtr=NULL;
	struct PIDCtrl *pidPitchPtr=NULL;
	struct PIDCtrl *pidYawPtr=NULL;
	
	int fd1,fd2,sPort2;
	int nbytes;
	long int counter;
	int semiAutomaticAux=0;

	//Pointer Inicialization
	imuPacketPtr=&imuPacket;
	servosActionPtr=&servosAction;
	imuShortPacketPtr=&imuShortPacket;
	
	//pthread_mutex_lock(&mutexPid);
	pidRollPtr=&pidRoll;
	pidPitchPtr=&pidPitch;
	pidYawPtr=&pidYaw;

	  
	/*********************************************************************
	*Open and configure USB Serial Port2 (IMU)
	*********************************************************************/
	sPort2 = open_serial(SERUSB_PORT2,BAUDRATE_115200);
	// Initial Request
	imuRequest(sPort2);	
	// Delay for Syncro
	wait.tv_sec=1;
	wait.tv_nsec=0;
	nanosleep(&wait,NULL);
	
	
	//----------------------------------------------- 
	// Main Loop
	//----------------------------------------------- 
	clock_gettime(CLOCK_REALTIME, &t0_ant);
	printf ("Attitude Control Task on-line.\n");
	counter = 0;

	while(1) 
	{
				
		// Getting Time Stamp
		clock_gettime (CLOCK_REALTIME, &t0);
		
		//Reading IMU
		imuAcquisition(sPort2,imuPacketPtr);
		
		// Clear Buffer
		tcflush(sPort2, TCIFLUSH);
		
		// Request Next Value
		imuRequest(sPort2);	
		
		// Time Calculation -> Next Activation
		next_activation=sum_tiempos(t0,Ts_attitudeCtrl); 
		
		// PIDs Signals Update
<<<<<<< .mine
		pthread_mutex_lock(&mutexPidLowLevel);
		pidRoll.Signals.y = imuPacket.roll;
		pidPitch.Signals.y = imuPacket.pitch;
		pthread_mutex_unlock(&mutexPidLowLevel);
		pthread_mutex_lock(&mutexGlobalVar);
		//-------- Sliding Angle for Yaw Control !!!!-------
		pidYaw.Signals.y = atan2f(-(imuPacket.T11*rotMatYawHeading.vn+imuPacket.T12*rotMatYawHeading.ve+imuPacket.T13*rotMatYawHeading.vd),(imuPacket.T21*rotMatYawHeading.vn+imuPacket.T22*rotMatYawHeading.ve+imuPacket.T23*rotMatYawHeading.vd));
		//-------- PID SIGNAL FOR HEADING !!!!-------
		rotMatYawHeading.Heading = imuPacket.yaw;
		//printf("Yaw: %f",imuPacket.yaw);
		pthread_mutex_unlock(&mutexGlobalVar);
		//-------- Check SemiAutomatic or Automatic !!!!-------
		pthread_mutex_lock(&mutexMachineState);
		semiAutomaticAux = semiAutomatic;
		pthread_mutex_unlock(&mutexMachineState);
		
=======
		pthread_mutex_lock(&mutexPidLowLevel);
		pidRoll.Signals.y = imuPacket.roll;
		pidPitch.Signals.y = imuPacket.pitch;
		pthread_mutex_lock(&mutexGlobalVar);
		//-------- Sliding Angle for Yaw Control !!!!-------
		pidYaw.Signals.y = atan2f(-(imuPacket.T11*rotMatYawHeading.vn+imuPacket.T12*rotMatYawHeading.ve+imuPacket.T13*rotMatYawHeading.vd),(imuPacket.T21*rotMatYawHeading.vn+imuPacket.T22*rotMatYawHeading.ve+imuPacket.T23*rotMatYawHeading.vd));
		//-------- PID SIGNAL FOR HEADING !!!!-------
		rotMatYawHeading.Heading = imuPacket.yaw;
		//printf("Yaw: %f",imuPacket.yaw);
		pthread_mutex_unlock(&mutexGlobalVar);
		//-------- Check SemiAutomatic or Automatic !!!!-------
		pthread_mutex_lock(&mutexMachineState);
		semiAutomaticAux = semiAutomatic;
		pthread_mutex_unlock(&mutexMachineState);

>>>>>>> .r976
		pthread_mutex_lock(&mutexPidLowLevel);
		// PID Roll Control
		PIDCalculateOutput(pidRollPtr,-1);
		PIDUpdateStates(pidRollPtr,semiAutomaticAux);
		
		// PID Pitch Control
<<<<<<< .mine
		PIDCalculateOutput(pidPitchPtr,1);
		PIDUpdateStates(pidPitchPtr,semiAutomaticAux);
=======
		PIDCalculateOutput(pidPitchPtr,-1);
		PIDUpdateStates(pidPitchPtr,semiAutomaticAux);
>>>>>>> .r976
		
		// PID Yaw Control
<<<<<<< .mine
		//PIDCalculateOutput(pidYawPtr,1);
		//PIDUpdateStates(pidYawPtr,semiAutomaticAux);
		pidYaw.Signals.u = 0.7*(pidRoll.Signals.u);
=======
		PIDCalculateOutput(pidYawPtr,1);
		PIDUpdateStates(pidYawPtr,semiAutomaticAux);
>>>>>>> .r976
		
		// Write Servo Actions from the PIDs
<<<<<<< .mine
		servosAction.roll = (short int)(((pidRoll.Signals.u*R2D)*21.5)+1500);
		//printf("Action Roll\n",servosAction.roll);
		servosAction.pitch = (short int)(((pidPitch.Signals.u*R2D)*21.5)+1500);
		servosAction.yaw = (short int)(((pidYaw.Signals.u*R2D)*21.5)+1500);
=======
		servosAction.roll = (short int)(((pidRoll.Signals.u*R2D)*21.5)+1500);
		//printf("Action Roll\n",servosAction.roll);
		servosAction.pitch = (short int)(((pidPitch.Signals.u*R2D)*21.5)+1500);
		servosAction.yaw = (short int)(((pidYaw.Signals.u)*21.5)+1500);
>>>>>>> .r976
		servosAction.thrust = pidVelocity.Signals.u;
		pthread_mutex_unlock(&mutexPidLowLevel);
		
		// Sending Control Actions - Regulat Jitter
		sendActions(servosActionPtr);
		
		// Save Data in FIFOs
		pthread_mutex_lock(&mutexPidLowLevel);
		counter = counter + 1;
		imuShortPacket.pitch = imuPacket.pitch;
		imuShortPacket.Refpitch = pidPitch.Signals.R;
		imuShortPacket.upitch = pidPitch.Signals.u;
		
		imuShortPacket.roll = imuPacket.roll;
		imuShortPacket.Refroll = pidRoll.Signals.R;
		imuShortPacket.uroll = pidRoll.Signals.u;
		
		imuShortPacket.yaw = imuPacket.yaw;
		imuShortPacket.Refpitch = pidYaw.Signals.R;
		imuShortPacket.upitch = pidYaw.Signals.u;

		imuShortPacket.Ts = ((int)Period.tv_nsec)/prec;
		imuShortPacket.counter = counter;
		pthread_mutex_unlock(&mutexPidLowLevel);
		
		// Datalogging FIFO
		fd1 = open(fifoAttiDatalog,O_WRONLY | O_NONBLOCK);
		if (fd1>0){
			
			nbytes = write(fd1,imuShortPacketPtr,sizeof(struct imuShort));
			//printf("[attitude FIFO write] %d \n",nbytes);
			close(fd1);
		}
		
		// Telemetry FIFO
		fd2 = open(fifoAttiTele,O_WRONLY | O_NONBLOCK);
		if (fd2>0){
		
			nbytes = write(fd2,imuShortPacketPtr,sizeof(struct imuShort));
			//printf("[attitude FIFO write] %d \n",nbytes);
			close(fd2);
		}
		
		
		//printf("IMU %3f %3f \n",imuPacket.ax,imuPacket.ay);
		//printf("[Euler Angles] %3f %3f %3f \n",imuPacket.roll*R2D,imuPacket.pitch*R2D,imuPacket.yaw*R2D);
		
		// Print the results
		//printf("A %3d %3d \n",((int)WCCT.tv_nsec)/prec,  ((int)Period.tv_nsec)/prec);
		
		// Wait until the next activation and Period Calculation
		Period = diff_tiempos(ID_navigationCtrl,t0,t0_ant,1);
		t0_ant = t0;
		//if (((((int)Period.tv_nsec)/prec)/1000)>41)
		//	printf("Attitude losing Deadline: %d \n",((((int)Period.tv_nsec)/prec)/1000));

		clock_gettime (CLOCK_REALTIME, &t1);
		if (timespec2nsec(&next_activation)>timespec2nsec(&t1))
		{
		    auxtime = (timespec2nsec(&next_activation)-timespec2nsec(&t1));
			nsec2timespec(&wait,auxtime);
			nanosleep(&wait, NULL);
		}	

	}
	 /*********************************************************************
	   *Close USB Serial Port2 (IMU)
	  *********************************************************************/
	  close(sPort2);
	return(NULL);
}
